
#include <cstdio>
#include <BSphere>
#include "IGjkEpaSolver.h"
#include "member_BShape.h"

#ifdef REAL_IS_DOUBLE
#define GJK_ACCURACY ((BReal)1e-12)
#define GJK_MIN_DISTANCE ((BReal)1e-12)
#define GJK_DUPLICATED_EPS ((BReal)1e-12)
#else
#define GJK_ACCURACY ((BReal)0.0001)
#define GJK_MIN_DISTANCE ((BReal)0.0001)
#define GJK_DUPLICATED_EPS ((BReal)0.0001)
#endif

#ifdef REAL_IS_DOUBLE
#define EPA_ACCURACY ((BReal)1e-12)
#define EPA_PLANE_EPS ((BReal)1e-14)
#define EPA_INSIDE_EPS ((BReal)1e-9)
#else
#define EPA_ACCURACY ((BReal)0.0001)
#define EPA_PLANE_EPS ((BReal)0.00001)
#define EPA_INSIDE_EPS ((BReal)0.01)
#endif

struct Vertex
{
	BVector direct;
	BVector vector;
};

struct Simplex
{
	Vertex*	vertex[4] = { 0 };
	BReal	weight[4] = { 0 };
	int		rank = 0;
};

struct GJK
{
	enum Status
	{
		Valid,
		Inside,
		Failed
	};

	BShape*		_shapeA;
	BShape*		_shapeB;
	BMatrix		_matrixA;
	BMatrix		_matrixB;
	BVector		_direct;
	BReal		_distance;
	Simplex		_simplices[2];
	Vertex		_store[4];
	Vertex*		_free[4];
	int			_numfree;
	int			_current;
	Simplex*	_simplex;
	Status		_status;

	inline GJK(BShape* shape0, const BMatrix& wtrs0, BShape* shape1, const BMatrix& wtrs1, IGjkEpaSolver::Result& results)
	{
		_shapeA = shape0;
		_shapeB = shape1;
		_matrixA = wtrs0;
		_matrixB = wtrs1;

		_free[0] = &_store[0];
		_free[1] = &_store[1];
		_free[2] = &_store[2];
		_free[3] = &_store[3];

		_numfree = 4;
		_current = 0;
		_simplex = 0;
		_status = Status::Valid;
		_distance = 0;

	}

	inline Status evaluate(const BVector& guess)
	{
		_simplices[0].rank = 0;
		BReal len2 = guess.length2();
		if (len2 > 0)
			appendVertice(_simplices[0], -guess);
		else
			appendVertice(_simplices[0], BVector(1, 0, 0));
		_simplices[0].weight[0] = 1;
		_direct = _simplices[0].vertex[0]->vector;

		BReal alpha = 0;
		BVector lastDirect[4];
		lastDirect[0] = _direct;
		lastDirect[1] = _direct;
		lastDirect[2] = _direct;
		lastDirect[3] = _direct;

		int clastw = 0;
		int iterations = 0;
		while (_status == Status::Valid)
		{
			const int next = 1 - _current;
			Simplex& simplex = _simplices[_current];
			Simplex& nextSimplex = _simplices[next];

			const BReal length = _direct.length();
			if (length < GJK_MIN_DISTANCE)
			{
				_status = Status::Inside;
				break;
			}

			appendVertice(simplex, -_direct);
			const BVector& vector = simplex.vertex[simplex.rank - 1]->vector;
			bool found = false;
			for (int i = 0; i < 4; ++i)
			{
				const BVector& direct = lastDirect[i];
				if ((vector - direct).length2() < GJK_DUPLICATED_EPS)
				{
					found = true;
					break;
				}
			}
			if (found)
			{
				removeVertice(_simplices[_current]);
				break;
			}
			clastw = (clastw + 1) & 3;
			lastDirect[clastw] = vector;

			const BReal omega = _direct.dot(vector) / length;
			alpha = bMax(omega, alpha);
			if ((length - alpha) - (GJK_ACCURACY * length) <= 0)
			{
				removeVertice(_simplices[_current]);
				break;
			}

			BReal weights[4] = { 0, 0, 0, 0 };
			int mask = 0;
			switch (simplex.rank)
			{
			case 2:
				len2 = projectOrigin(simplex.vertex[0]->vector, simplex.vertex[1]->vector, weights, mask);
				break;
			case 3:
				len2 = projectOrigin(simplex.vertex[0]->vector, simplex.vertex[1]->vector, simplex.vertex[2]->vector, weights, mask);
				break;
			case 4:
				len2 = projectOrigin(simplex.vertex[0]->vector, simplex.vertex[1]->vector, simplex.vertex[2]->vector, simplex.vertex[3]->vector, weights, mask);
				break;
			}
			if (len2 >= 0)
			{
				nextSimplex.rank = 0;
				_direct.reset();
				_current = next;
				for (int i = 0; i < simplex.rank; ++i)
				{
					if (mask & (1 << i))
					{
						nextSimplex.vertex[nextSimplex.rank] = simplex.vertex[i];
						nextSimplex.weight[nextSimplex.rank++] = weights[i];
						_direct += simplex.vertex[i]->vector * weights[i];
					}
					else
					{
						_free[_numfree++] = simplex.vertex[i];
					}
				}
				if (mask == 15)
					_status = Status::Inside;
			}
			else
			{
				removeVertice(_simplices[_current]);
				break;
			}
			if (iterations++ > 128)
			{
				_status = Status::Failed;
				break;
			}
		}
		_simplex = _simplices + _current;
		if (_status == Status::Valid)
			_distance = _direct.length();
		return _status;
	}
	inline bool encloseOrigin()
	{
		switch (_simplex->rank)
		{
		case 1:
		{
			for (int i = 0; i < 3; ++i)
			{
				BVector axis = BVector(0, 0, 0);
				axis[i] = 1;
				appendVertice(*_simplex, axis);
				if (encloseOrigin())
					return true;
				removeVertice(*_simplex);
				appendVertice(*_simplex, -axis);
				if (encloseOrigin())
					return true;
				removeVertice(*_simplex);
			}
			break;
		}
		case 2:
		{
			const BVector d = _simplex->vertex[1]->vector - _simplex->vertex[0]->vector;
			for (int i = 0; i < 3; ++i)
			{
				BVector axis = BVector(0, 0, 0);
				axis[i] = 1;
				const BVector p = d.cross(axis);
				if (p.length2() > 0)
				{
					appendVertice(*_simplex, p);
					if (encloseOrigin())
						return true;
					removeVertice(*_simplex);
					appendVertice(*_simplex, -p);
					if (encloseOrigin())
						return true;
					removeVertice(*_simplex);
				}
			}
			break;
		}
		case 3:
		{
			const BVector n = (_simplex->vertex[1]->vector - _simplex->vertex[0]->vector).cross(_simplex->vertex[2]->vector - _simplex->vertex[0]->vector);
			if (n.length2() > 0)
			{
				appendVertice(*_simplex, n);
				if (encloseOrigin())
					return true;
				removeVertice(*_simplex);
				appendVertice(*_simplex, -n);
				if (encloseOrigin())
					return true;
				removeVertice(*_simplex);
			}
			break;
		}
		case 4:
		{
			BReal d = det(
				_simplex->vertex[0]->vector - _simplex->vertex[3]->vector,
				_simplex->vertex[1]->vector - _simplex->vertex[3]->vector,
				_simplex->vertex[2]->vector - _simplex->vertex[3]->vector);
			if (fabs(d) > 0)
				return true;
			break;
		}
		}
		return false;
	}

	inline BVector supportA(const BVector& d) const
	{
		return shape_member(_shapeA)->supportVertex(d);
	}
	inline BVector supportB(const BVector& d) const
	{
		BVector dir = _matrixA.mult3(d);
		dir = _matrixB.inverse().mult3(dir);
		BVector vec = _matrixB * shape_member(_shapeB)->supportVertex(dir);
		return _matrixA.inverse() * vec;
	}

	inline void removeVertice(Simplex& simplex)
	{
		_free[_numfree++] = simplex.vertex[--simplex.rank];
	}
	inline void appendVertice(Simplex& simplex, const BVector& direct)
	{
		simplex.weight[simplex.rank] = 0;
		simplex.vertex[simplex.rank] = _free[--_numfree];
		Vertex* vertex = simplex.vertex[simplex.rank++];
		vertex->direct = direct.normal();
		vertex->vector = supportA(vertex->direct) - supportB(-vertex->direct);
	}

	static BReal det(const BVector& a, const BVector& b, const BVector& c)
	{
		return
			a.y() * b.z() * c.x() + a.z() * b.x() * c.y() -
			a.x() * b.z() * c.y() - a.y() * b.x() * c.z() +
			a.x() * b.y() * c.z() - a.z() * b.y() * c.x();
	}

	static BReal projectOrigin(const BVector& a, const BVector& b, BReal* weights, int& mask)
	{
		const BVector d = b - a;
		const BReal l = d.length2();
		if (l > BReal(0))
		{
			const BReal t(l > 0 ? -a.dot(d) / l : 0);
			if (t >= 1)
			{
				weights[0] = 0;
				weights[1] = 1;
				mask = 2;
				return (b.length2());
			}
			else if (t <= 0)
			{
				weights[0] = 1;
				weights[1] = 0;
				mask = 1;
				return (a.length2());
			}
			else
			{
				weights[0] = 1 - t;
				weights[1] = t;
				mask = 3;
				return ((a + d * t).length2());
			}
		}
		return (-1);
	}
	static BReal projectOrigin(const BVector& a, const BVector& b, const BVector& c, BReal* weights, int& mask)
	{
		static const int imd3[] = { 1, 2, 0 };
		const BVector* vt[] = { &a, &b, &c };
		const BVector dl[] = { a - b, b - c, c - a };
		const BVector n = dl[0].cross(dl[1]);
		const BReal l = n.length2();
		if (l > BReal(0))
		{
			BReal mindist = -1;
			BReal subw[2] = { 0.f, 0.f };
			int subm(0);
			for (int i = 0; i < 3; ++i)
			{
				if ((*vt[i]).dot(dl[i].cross(n)) > 0)
				{
					const int j = imd3[i];
					const BReal subd = projectOrigin(*vt[i], *vt[j], subw, subm);
					if ((mindist < 0) || (subd < mindist))
					{
						mindist = subd;
						mask = ((subm & 1) ? 1 << i : 0) + ((subm & 2) ? 1 << j : 0);
						weights[i] = subw[0];
						weights[j] = subw[1];
						weights[imd3[j]] = 0;
					}
				}
			}
			if (mindist < 0)
			{
				const BReal d = a.dot(n);
				const BReal s = sqrt(l);
				const BVector p = n * (d / l);
				mindist = p.length2();
				mask = 7;
				weights[0] = (dl[1].cross(b - p)).length() / s;
				weights[1] = (dl[2].cross(c - p)).length() / s;
				weights[2] = 1 - (weights[0] + weights[1]);
			}
			return (mindist);
		}
		return -1;
	}
	static BReal projectOrigin(const BVector& a, const BVector& b, const BVector& c, const BVector& d, BReal* weights, int& mask)
	{
		static const int imd3[] = { 1, 2, 0 };
		const BVector* vt[] = { &a, &b, &c, &d };
		const BVector dl[] = { a - d, b - d, c - d };
		const BReal vl = det(dl[0], dl[1], dl[2]);
		const bool ng = (vl * a.dot((b - c).cross(a - b))) <= 0;
		if (ng && (fabs(vl) > BReal(0)))
		{
			BReal mindist = -1;
			BReal subw[3] = { 0.f, 0.f, 0.f };
			int subm(0);
			for (int i = 0; i < 3; ++i)
			{
				const int j = imd3[i];
				const BReal s = vl * d.dot(dl[i].cross(dl[j]));
				if (s > 0)
				{
					const BReal subd = projectOrigin(*vt[i], *vt[j], d, subw, subm);
					if ((mindist < 0) || (subd < mindist))
					{
						mindist = subd;
						mask = (
							(subm & 1 ? 1 << i : 0) +
							(subm & 2 ? 1 << j : 0) +
							(subm & 4 ? 8 : 0));
						weights[i] = subw[0];
						weights[j] = subw[1];
						weights[imd3[j]] = 0;
						weights[3] = subw[2];
					}
				}
			}
			if (mindist < 0)
			{
				mindist = 0;
				mask = 15;
				weights[0] = det(c, b, d) / vl;
				weights[1] = det(a, c, d) / vl;
				weights[2] = det(b, a, d) / vl;
				weights[3] = 1 - (weights[0] + weights[1] + weights[2]);
			}
			return (mindist);
		}
		return (-1);
	}
};

struct EPA
{
	struct Face
	{
		BVector normal;
		BReal depth = 0;
		Vertex* vertex[3];
		Face* face[3];
		Face* list[2];
		int e[3];
		int pass = 0;
	};
	struct List
	{
		Face* root = 0;
		int count = 0;
	};
	struct Horizon
	{
		Face* cf = 0;
		Face* ff = 0;
		int nf = 0;
	};
	enum Status
	{
		Valid,
		Touching,
		Degenerated,
		NonConvex,
		InvalidHull,
		OutOfFaces,
		OutOfVertices,
		AccuraryReached,
		FallBack,
		Failed
	};

	Status	_status;
	Simplex _result;
	BVector _normal;
	BReal	_depth;
	Vertex	_vertexs[128];
	Face	_faces[128];
	List	_hull;
	List	_stock;

	static inline void bind(Face* fa, int ea, Face* fb, int eb)
	{
		fa->e[ea] = eb;
		fa->face[ea] = fb;
		fb->e[eb] = ea;
		fb->face[eb] = fa;
	}
	static inline void append(List& list, Face* face)
	{
		face->list[0] = 0;
		face->list[1] = list.root;
		if (list.root)
			list.root->list[0] = face;
		list.root = face;
		++list.count;
	}
	static inline void remove(List& list, Face* face)
	{
		if (face->list[1])
			face->list[1]->list[0] = face->list[0];
		if (face->list[0])
			face->list[0]->list[1] = face->list[1];
		if (face == list.root)
			list.root = face->list[1];
		--list.count;
	}

	EPA()
	{
		_status = Status::Failed;
		_depth = 0;
		for (int i = 0; i < 128; ++i)
		{
			append(_stock, &_faces[128 - i - 1]);
		}
	}
	Status evaluate(GJK& gjk, const BVector& direct)
	{
		Simplex* simplex = gjk._simplex;
		if (simplex->rank > 1 && gjk.encloseOrigin())
		{
			while (_hull.root)
			{
				Face* face = _hull.root;
				remove(_hull, face);
				append(_stock, face);
			}
			_status = Status::Valid;
			if (gjk.det(
				simplex->vertex[0]->vector - simplex->vertex[3]->vector,
				simplex->vertex[1]->vector - simplex->vertex[3]->vector,
				simplex->vertex[2]->vector - simplex->vertex[3]->vector) < 0)
			{
				bSwap(simplex->vertex[0], simplex->vertex[1]);
				bSwap(simplex->weight[0], simplex->weight[1]);
			}
			Face* face0 = newFace(simplex->vertex[0], simplex->vertex[1], simplex->vertex[2], true);
			Face* face1 = newFace(simplex->vertex[1], simplex->vertex[0], simplex->vertex[3], true);
			Face* face2 = newFace(simplex->vertex[2], simplex->vertex[1], simplex->vertex[3], true);
			Face* face3 = newFace(simplex->vertex[0], simplex->vertex[2], simplex->vertex[3], true);

			if (_hull.count == 4)
			{
				bind(face0, 0, face1, 0);
				bind(face0, 1, face2, 0);
				bind(face0, 2, face3, 0);
				bind(face1, 1, face3, 2);
				bind(face1, 2, face2, 1);
				bind(face2, 2, face3, 1);

				Face* best = findBest();
				int pass = 0;
				int next = 0;
				int iterations = 0;
				while (iterations++ < 255)
				{
					if (next < 128)
					{
						Horizon horizon;
						Vertex* vertex = &_vertexs[next++];
						best->pass = ++pass;
						vertex->direct = best->normal.normal();
						vertex->vector = gjk.supportA(vertex->direct) - gjk.supportB(-vertex->direct);
						const BReal dist = best->normal.dot(vertex->vector) - best->depth;
						if (dist > EPA_ACCURACY)
						{
							bool valid = true;
							for (int j = 0; j < 3 && valid; ++j)
							{
								valid &= expand(pass, vertex, best->face[j], best->e[j], horizon);
							}
							if (valid && (horizon.nf >= 3))
							{
								bind(horizon.cf, 1, horizon.ff, 2);
								remove(_hull, best);
								append(_stock, best);
								best = findBest();
							}
							else
							{
								_status = Status::InvalidHull;
								break;
							}
						}
						else
						{
							_status = Status::AccuraryReached;
							break;
						}
					}
					else
					{
						_status = Status::OutOfVertices;
						break;
					}
				}
				const BVector projection = best->normal * best->depth;
				_normal = best->normal;
				_depth = best->depth;
				_result.rank = 3;
				_result.vertex[0] = best->vertex[0];
				_result.vertex[1] = best->vertex[1];
				_result.vertex[2] = best->vertex[2];
				_result.weight[0] = (best->vertex[1]->vector - projection).cross(best->vertex[2]->vector - projection).length();
				_result.weight[1] = (best->vertex[2]->vector - projection).cross(best->vertex[0]->vector - projection).length();
				_result.weight[2] = (best->vertex[0]->vector - projection).cross(best->vertex[1]->vector - projection).length();
				const BReal sum = _result.weight[0] + _result.weight[1] + _result.weight[2];
				_result.weight[0] /= sum;
				_result.weight[1] /= sum;
				_result.weight[2] /= sum;
				return _status;
			}
		}

		_status = Status::FallBack;
		_normal = -direct;
		const BReal length = _normal.length();
		if (length > 0)
			_normal = _normal / length;
		else
			_normal = BVector(1, 0, 0);
		_depth = 0;
		_result.rank = 1;
		_result.vertex[0] = simplex->vertex[0];
		_result.weight[0] = 1;
		return _status;
	}
	bool edgeDist(Face* face, Vertex* a, Vertex* b, BReal& dist)
	{
		const BVector ba = b->vector - a->vector;
		const BVector n_ab = ba.cross(face->normal);// Outward facing edge normal direction, on triangle plane
		const BReal a_dot_nab = a->vector.dot(n_ab);// Only care about the sign to determine inside/outside, so not normalization required
		if (a_dot_nab < 0)
		{
			// Outside of edge a->b
			const BReal ba_l2 = ba.length2();
			const BReal a_dot_ba = a->vector.dot(ba);
			const BReal b_dot_ba = b->vector.dot(ba);
			if (a_dot_ba > 0)
			{
				// Pick distance vertex a
				dist = a->vector.length();
			}
			else if (b_dot_ba < 0)
			{
				// Pick distance vertex b
				dist = b->vector.length();
			}
			else
			{
				// Pick distance to edge a->b
				const BReal a_dot_b = a->vector.dot(b->vector);
				dist = sqrt(bMax((a->vector.length2() * b->vector.length2() - a_dot_b * a_dot_b) / ba_l2, (BReal)0));
			}
			return true;
		}
		return false;
	}
	Face* newFace(Vertex* a, Vertex* b, Vertex* c, bool forced)
	{
		if (_stock.root)
		{
			Face* face = _stock.root;
			remove(_stock, face);
			append(_hull, face);
			face->pass = 0;
			face->vertex[0] = a;
			face->vertex[1] = b;
			face->vertex[2] = c;
			face->normal = (b->vector - a->vector).cross(c->vector - a->vector);
			const BReal len = face->normal.length();
			const bool v = len > EPA_ACCURACY;
			if (v)
			{
				if (!(edgeDist(face, a, b, face->depth) || edgeDist(face, b, c, face->depth) || edgeDist(face, c, a, face->depth)))
				{
					// Origin projects to the interior of the triangle
					// Use distance to triangle plane
					face->depth = a->vector.dot(face->normal) / len;
				}

				face->normal /= len;
				if (forced || (face->depth >= -EPA_PLANE_EPS))
					return face;
				_status = Status::NonConvex;
			}
			else
			{
				_status = Status::Degenerated;
			}
			remove(_hull, face);
			append(_stock, face);
			return 0;
		}
		_status = _stock.root ? Status::OutOfVertices : Status::OutOfFaces;
		return 0;
	}
	Face* findBest()
	{
		Face* minf = _hull.root;
		BReal mind = minf->depth * minf->depth;
		for (Face* face = minf->list[1]; face; face = face->list[1])
		{
			const BReal sqd = face->depth * face->depth;
			if (sqd < mind)
			{
				minf = face;
				mind = sqd;
			}
		}
		return (minf);
	}
	bool expand(int pass, Vertex* ssv, Face* face, int e, Horizon& horizon)
	{
		static const int i1m3[] = { 1, 2, 0 };
		static const int i2m3[] = { 2, 0, 1 };
		if (face->pass != pass)
		{
			const int e1 = i1m3[e];
			if ((face->normal.dot(ssv->vector) - face->depth) < -EPA_PLANE_EPS)
			{
				Face* nf = newFace(face->vertex[e1], face->vertex[e], ssv, false);
				if (nf)
				{
					bind(nf, 0, face, e);
					if (horizon.cf)
						bind(horizon.cf, 1, nf, 2);
					else
						horizon.ff = nf;
					horizon.cf = nf;
					++horizon.nf;
					return true;
				}
			}
			else
			{
				const int e2 = i2m3[e];
				face->pass = pass;
				if (expand(pass, ssv, face->face[e1], face->e[e1], horizon) &&
					expand(pass, ssv, face->face[e2], face->e[e2], horizon))
				{
					remove(_hull, face);
					append(_stock, face);
					return true;
				}
			}
		}
		return false;
	}
};

bool IGjkEpaSolver::Distance(BShape* shape0, const BMatrix& matrixA, BShape* shape1, const BMatrix& matrixB, const BVector& guess, Result& results)
{
	GJK gjk(shape0, matrixA, shape1, matrixB, results);
	GJK::Status gjk_status = gjk.evaluate(guess);
	if (gjk_status == GJK::Status::Valid)
	{
		BVector pointOnA;
		BVector pointOnB;
		for (int i = 0; i < gjk._simplex->rank; ++i)
		{
			const BReal& weight = gjk._simplex->weight[i];
			const BVector& direct = gjk._simplex->vertex[i]->direct;
			pointOnA += gjk.supportA(direct) * weight;
			pointOnB += gjk.supportB(-direct) * weight;
		}
		results.pointA = matrixA * pointOnA;
		results.pointB = matrixA * pointOnB;
		results.normal = pointOnA - pointOnB;
		results.distance = results.normal.length();
		results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
		return true;
	}
	else
	{
		return false;
	}
}

bool IGjkEpaSolver::Penetration(BShape* shapeA, const BMatrix& matrixA, BShape* shapeB, const BMatrix& matrixB, const BVector& direct, Result& results)
{
	GJK gjk(shapeA, matrixA, shapeB, matrixB, results);
	GJK::Status gjk_status = gjk.evaluate(-direct);
	if (gjk_status == GJK::Status::Inside)
	{
		EPA epa;
		EPA::Status status = epa.evaluate(gjk, -direct);
		if (status != EPA::Status::Failed)
		{
			BVector pointOnA;
			for (int i = 0; i < epa._result.rank; ++i)
			{
				const BReal& weight = epa._result.weight[i];
				const BVector& direct = epa._result.vertex[i]->direct;
				pointOnA += gjk.supportA(direct) * weight;
			}
			results.pointA = matrixA * pointOnA;
			results.pointB = matrixA * (pointOnA - epa._normal * epa._depth);
			results.normal = -epa._normal;
			results.distance = -epa._depth;
			return true;
		}
	}
	return false;
}

BReal IGjkEpaSolver::SignedDistance(const BVector& position, BReal margin, BShape* shapeA, const BMatrix& matrixA, Result& results)
{
	BSphere shape1(margin);
	BMatrix wtrs1(BQuater(0, 0, 0, 1), position);
	GJK gjk(shapeA, matrixA, &shape1, wtrs1, results);
	GJK::Status gjk_status = gjk.evaluate(BVector(1, 1, 1));
	if (gjk_status == GJK::Status::Valid)
	{
		BVector pointOnA;
		BVector pointOnB;
		for (int i = 0; i < gjk._simplex->rank; ++i)
		{
			const BReal& weight = gjk._simplex->weight[i];
			const BVector& direct = gjk._simplex->vertex[i]->direct;
			pointOnA += gjk.supportA(direct) * weight;
			pointOnB += gjk.supportB(-direct) * weight;
		}
		results.pointA = matrixA * pointOnA;
		results.pointB = matrixA * pointOnB;
		const BVector delta = results.pointB - results.pointA;
		const BReal margin = shapeA->margin() + shape1.margin();
		const BReal length = delta.length();
		results.normal = delta / length;
		results.pointA += results.normal * margin;
		results.distance = length - margin;
		return results.distance;
	}
	else
	{
		if (gjk_status == GJK::Status::Inside)
		{
			if (Penetration(shapeA, matrixA, &shape1, wtrs1, gjk._direct, results))
			{
				const BVector delta = results.pointA - results.pointB;
				const BReal length = delta.length();
				if (length >= REAL_EPSILON)
					results.normal = delta / length;
				return (-length);
			}
		}
	}
	return (REAL_MAX);
}

bool IGjkEpaSolver::SignedDistance(BShape* shape0, const BMatrix& wtrs0, BShape* shape1, const BMatrix& wtrs1, const BVector& guess, Result& results)
{
	if (!Distance(shape0, wtrs0, shape1, wtrs1, guess, results))
		return (Penetration(shape0, wtrs0, shape1, wtrs1, guess, results));
	else
		return (true);
}
